﻿using System;
using System.Linq;
using Microsoft.Xna.Framework;

namespace GameFrameworkLibraly.Helpers.GeometryHelpers
{
    public static class GeometryHelper
    {
        /// <summary>
        /// Calculates signed angle between 2D Vecors
        /// </summary>
        /// <param name="fromVector">Vector to start at.</param>
        /// <param name="destVector">Destination vector</param>
        /// <param name="destVectorsRight">Vector that shows right direction (in general - (1,0))</param>
        /// <returns>Returns positive angle if rotated toward the clock's arrow</returns>
        public static double GetSignedAngleBetween2DVectors(Vector2 fromVector, Vector2 destVector, Vector2 destVectorsRight)
        {
            fromVector.Normalize();
            destVector.Normalize();
            destVectorsRight.Normalize();

            float forwardDot = Vector2.Dot(fromVector, destVector);

            // Keep dot in range to prevent rounding errors
            forwardDot = MathHelper.Clamp(forwardDot, -1.0f, 1.0f);

            double angleBetween = Math.Acos(forwardDot);

            if (Vector2.Dot(fromVector, destVectorsRight) > Vector2.Dot(destVector, destVectorsRight))
                angleBetween *= -1.0f;

            return angleBetween;
        }


        /// <summary>
        /// Rotates a point around an origin
        /// </summary>
        /// <param name="point">Point that is going to be translated</param>
        /// <param name="originPoint">Origin of rotation</param>
        /// <param name="radiansToRotate">Radians to rotate</param>
        /// <returns>Rotated point</returns>
        public static Vector2 RotateAroundPoint(Vector2 point, Vector2 originPoint, float radiansToRotate)
        {
            Vector2 diffVect = point - originPoint;
            diffVect = new Vector2((float)(diffVect.X * Math.Cos(radiansToRotate) - diffVect.Y * Math.Sin(radiansToRotate)), (float)(diffVect.X * Math.Sin(radiansToRotate) + diffVect.Y * Math.Cos(radiansToRotate)));

            return diffVect + originPoint;
        }

        /// <summary>
        /// Point in polygon check.
        /// </summary>
        /// <param name="point">The point.</param>
        /// <param name="polygon">The polygon.</param>
        /// <returns>True if point is inside, false otherwise.</returns>
        public static bool PointInPolygon(this Point point, Polygon polygon)
        {

            bool inside = false;

            foreach (var side in polygon.Lines)
            {
                if (point.Y > Math.Min(side.Start.Y, side.End.Y))
                    if (point.Y <= Math.Max(side.Start.Y, side.End.Y))
                        if (point.X <= Math.Max(side.Start.X, side.End.X))
                        {
                            float xIntersection = side.Start.X +
                                                  ((point.Y - side.Start.Y)/(side.End.Y - side.Start.Y))*
                                                  (side.End.X - side.Start.X);
                            if (point.X <= xIntersection)
                                inside = !inside;

                        }
            }

            return inside;
        }


        /// <summary>
        /// Checks if 2 polygons intersect
        /// </summary>
        /// <param name="polygon">First polygon</param>
        /// <param name="polygon2">Second polygon</param>
        /// <returns>True if polygons intersect, false otherwise.</returns>
        public static bool IntersectsPoligone(this Polygon polygon, Polygon polygon2)
        {
            if (polygon.Vertexes.Any(vertex => vertex.PointInPolygon(polygon2)))
            {
                return true;
            }
            if (polygon2.Vertexes.Any(vertex => vertex.PointInPolygon(polygon)))
            {
                return true;
            }
            return false;
        }

        /// <summary>
        /// Calculates distance between 2 points
        /// </summary>
        /// <param name="p1">First point</param>
        /// <param name="p2">Second point</param>
        /// <returns>Distance</returns>
        public static double DistanceBetween2Points(this Point p1, Point p2)
        {
            return  Math.Sqrt((p1.X - p2.X)*(p1.X - p2.X) + (p1.Y - p2.Y)*(p1.Y - p2.Y));
        }

        /// <summary>
        /// Checks if point is in circle
        /// </summary>
        /// <param name="p1">Point</param>
        /// <param name="boundingRect">Bounding rectangle for circle</param>
        /// <returns>True if point is in circle, false otherwise</returns>
        public static bool PointInCircle(this Point p1, Rectangle boundingRect)
        {
            return DistanceBetween2Points(p1, boundingRect.Center) < boundingRect.Height/2;
        }
    }
}